#ifndef _PLL_H__
#define _PLL_H__

#include "utils/utils.h" // IWYU pragma: export
#include "math/fast_math.h"

typedef struct {
	float ts;
	float observer;
	float kp;
	float ki;
	float ui;
	float out;
	bool  ob_wp;
}pll_t;

__STATIC_INLINE void pll_reset(pll_t *pll, s32 sample) {
	pll->observer = sample;
	pll->out = 0;
	pll->ob_wp = false;
}

/*
通过matlab仿真，通过Wn计算Kp Ki，下面的公式得到的带宽，最接近设计带宽
(先通过二阶巴特沃兹滤波器初步设计，然后调整Ki)
Wn = 50 * 2 * pi;
Kp = 0.707 * Wn;
Ki = Wn * Wn / 4.427;
Gclose = tf([Kp Ki], [1 Kp Ki]);
bandwidth(Gclose)
Cclose = c2d(Gclose, 60e-6);
bandwidth(Cclose)
bodeplot(Gclose, Cclose);
*/

__STATIC_INLINE void pll_set_bandwidth(pll_t *pll, float Wn) {
	pll->kp = 0.707f * Wn;
	pll->ki = Wn * Wn / 4.427f;
}

__STATIC_INLINE float pll_run(pll_t *pll, float sample, float saturation) {
	float delta = sample - pll->observer;
	if (delta < -saturation/2.0f) {
		delta += saturation;
		pll->observer -= saturation;
	}else if (delta > saturation/2.0f) {
		delta -= saturation;
		pll->observer += saturation;
	}
	pll->observer = pll->observer + (pll->out + pll->kp * delta) * pll->ts;
	pll->out += pll->ki * delta * pll->ts;
	return pll->out;
}

#endif /* _PLL_H__ */
